package com.neuronrobotics.replicator.gui.preview.controller;

import java.util.Arrays;

import javax.vecmath.AxisAngle4f;
import javax.vecmath.Matrix3f;
import javax.vecmath.Matrix4f;
import javax.vecmath.Point3f;
import javax.vecmath.Vector3f;
import javax.vecmath.Vector4f;

import com.neuronrobotics.replicator.gui.stl.GeneralTransform3D.MatrixOrder;

public class DefaultGeneralSTLTransformHelper implements GeneralSTLTransformHelper{
	
	final private float[] identityArray = new float[]{1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1};
	
	private float[] currentTransform;
	private Matrix4f currentMatrix;	
	
	public DefaultGeneralSTLTransformHelper(){
		currentTransform = Arrays.copyOf(identityArray, 16);
		currentMatrix = new Matrix4f(currentTransform);
	}
	
	public DefaultGeneralSTLTransformHelper(float[] initialTransform){
		currentTransform = Arrays.copyOf(initialTransform,16);
		currentMatrix.set(currentTransform);
	}
	
	@Override
	public void rotateX(float angle) {
		Matrix4f temp = identityMatrix();
		temp.rotX(angle);
		currentMatrix.mul(temp);
		this.getDoubleArrayFromMatrix(currentMatrix, currentTransform);		
	}

	@Override
	public void rotateY(float angle) {
		Matrix4f temp = identityMatrix();
		temp.rotY(angle);
		currentMatrix.mul(temp);
		this.getDoubleArrayFromMatrix(currentMatrix, currentTransform);
	}

	@Override
	public void rotateZ(float angle) {
		Matrix4f temp = identityMatrix();
		temp.rotZ(angle);
		currentMatrix.mul(temp);
		this.getDoubleArrayFromMatrix(currentMatrix, currentTransform);
	}
	
	public void translate(float x, float y, float z) {
		
		//Log.d("MyTag","Pre-Translate: "+Arrays.toString(currentTransform));
		//Log.d("MyTag","Intended Translation: "+x+" "+y+" "+z);
		
		Matrix4f tempMat = identityMatrix();
		tempMat.setTranslation(new Vector3f(x,y,z));
		currentMatrix.mul(tempMat);
		this.getDoubleArrayFromMatrix(currentMatrix, currentTransform);
		//Log.d("MyTag","Post-Translate: "+Arrays.toString(currentTransform));
		
	}

	private Matrix4f identityMatrix() {
		return new Matrix4f(identityArray);
	}

	@Override
	public void translateLeftRight(float dist) {
		Vector3f tran = new Vector3f(1, 0, 0);
		Matrix4f curMat = new Matrix4f(currentTransform);
			
		
		try{
		curMat.invert();
		} catch(Exception e){
			System.out.println(curMat);
			throw new javax.vecmath.SingularMatrixException();
			//this blew up once?
		}
		
		curMat.transform(tran);
		tran.normalize();
		tran.scale(dist);
		translate(tran.x,tran.y,tran.z); 
	}

	@Override
	public void translateBackForth(float dist) {
		Vector3f tran = new Vector3f(0, 0, 1);
		Matrix4f curMat = new Matrix4f(currentTransform);
		curMat.invert();
			
		curMat.transform(tran);
		tran.normalize();
		tran.scale(dist);
		
		translate(tran.x,tran.y,tran.z);		
	}
		
	@Override
	public void translateLateral(float y, float[] camPos, float[] camLook) {
		Vector3f camDir = new Vector3f(camLook);	
		camDir.sub(new Vector3f(camPos));
		camDir.setY(0);
		
		Vector3f lat = new Vector3f();
		lat.cross(camDir, new Vector3f(0,1,0));
		lat.normalize();
		
		translateBackForth(lat.z*y);		
		translateLeftRight(lat.x*y);		
	}

	@Override
	public void translateInOut(float z, float[] camPos, float[] camLook) {
		Vector3f camDir = new Vector3f(camLook);	
		camDir.sub(new Vector3f(camPos));
		camDir.setY(0);
		camDir.normalize();		
					
		translateBackForth(-camDir.z*z);
		translateLeftRight(-camDir.x*z);	
		
	}

	@Override
	public void translateUpDown(float dist) {
		Vector3f tran = new Vector3f(0, 1, 0);
		
		Matrix4f curMat = new Matrix4f(currentTransform);
		
		//TODO scaling might make matrix non-invertible?
		//Matrix3f rotMat = new Matrix3f();
		//curMat.getRotationScale(rotMat);
		
		curMat.invert();

		curMat.transform(tran);
		
		//rotMat.invert();
		//rotMat.transform(tran);
		
		tran.normalize();
		tran.scale(dist);
		
		
		translate(tran.x,tran.y,tran.z);		
	}
	
	//TODO this shit don't work still
	public void newRotateAroundUpVector(float rad){

		Vector3f up = new Vector3f(0,1,0);
		
		//up.cross(up, reelUp);
		
		//Vector3f up2 = new Vector3f(up.x,up.y,up.z);
		
		Matrix4f invMat = new Matrix4f(currentMatrix);
		invMat.invert();
		invMat.transform(up);		
		
		Vector3f up2 = new Vector3f(up.x,up.y,up.z);
		
		up2.normalize();
		
		//System.out.println("Rotating around up vector: "+up2);
		
		float[] newUp = new float[3];
		up2.get(newUp);
		
		float[] w = new float[]{
				0,-newUp[2],newUp[1],0,
				newUp[2],0,-newUp[0],0,
				-newUp[1],newUp[0],0,0,
				0,0,0,1
				};
		
		//I+sin(theta)W+(2sin^2(theta/2)W^2
		float[] I = {
				1,0,0,0,
				0,1,0,0,
				0,0,1,0,
				0,0,0,1
		};
		float[] wA = Arrays.copyOf(w, 16);
		float[] wB = Arrays.copyOf(w,16);
		int ct=0;
		float sc = (float)Math.sin(rad);
		float sc2 = (float)(2*Math.sin(rad/2)*Math.sin(rad/2));
		for(float n:wA){
			wA[ct++]=n*sc;
		}
		ct=0;
		for(float n:wB){
			wB[ct]=wB[ct]*wB[ct]*sc2;
			ct++;
		}
		for(int i = 0;i<16;i++){
			I[i]=I[i]+wA[i]+wB[i];
		}
		
		Matrix4f r = new Matrix4f(currentTransform);

		Matrix4f i = new Matrix4f(I);
		
		r.mul(i);
		float[] temp = new float[4];
		float[] temp2 = new float[4];
		float[] temp3 = new float[4];
		float[] temp4 = new float[4];
		r.getRow(0, temp);
		r.getRow(1, temp2);
		r.getRow(2, temp3);
		r.getRow(3, temp4);
		currentTransform[0]=temp[0];
		currentTransform[1]=temp[1];
		currentTransform[2]=temp[2];
		currentTransform[3]=temp[3];
		
		currentTransform[4]=temp2[0];
		currentTransform[5]=temp2[1];
		currentTransform[6]=temp2[2];
		currentTransform[7]=temp2[3];
		
		currentTransform[8]=temp3[0];
		currentTransform[9]=temp3[1];
		currentTransform[10]=temp3[2];
		currentTransform[11]=temp3[3];
		
		currentTransform[12]=temp4[0];
		currentTransform[13]=temp4[1];
		currentTransform[14]=temp4[2];
		currentTransform[15]=1;
		
		this.currentMatrix = new Matrix4f(currentTransform);
	}
	
	//TODO old version
	public void rotateAroundUpVector(float rad){
				
		//TODO This don't work at all
		Vector3f up = new Vector3f(0,1,0);
				
		//up.cross(up, reelUp);
		
		//Vector3f up2 = new Vector3f(up.x,up.y,up.z);
		
		Matrix4f invMat = new Matrix4f(currentMatrix);
		invMat.invert();
		invMat.transform(up);		
		
		Vector3f up2 = new Vector3f(up.x,up.y,up.z);
		
		up2.normalize();
		
		System.out.println("Rotating around up vector: "+up2);
		
		AxisAngle4f axAngRot = new AxisAngle4f(up, rad);
				
		//currentMatrix.setRotation(axAngRot);
		//this.currentMatrix.setRotation(rotAxAng);
		//this.getDoubleArrayFromMatrix(currentMatrix, currentTransform);
		
		this.rotateX(axAngRot.x*rad);
		this.rotateY(axAngRot.y*rad);
		this.rotateZ(axAngRot.z*rad);		
		
	}
	
	@Override
	public void rotateAroundUpVector(float d, float[] center) {
		
		float[] centA = Arrays.copyOf(center, 3);
		
		transformPoint(centA);
		rotateAroundUpVector(d);
		float[] centB = Arrays.copyOf(center, 3);
				
		transformPoint(centB);
		
		float x=centA[0]-centB[0],z=centA[2]-centB[2];
		
		this.translateLeftRight(x);
		this.translateBackForth(z);
		
	}
			
	public void placeOnWorkspace(float workspaceY, float currMin){
		float epsilon = 0.0001f;
		this.translateUpDown(workspaceY-currMin+epsilon);
	}
		
	private AxisAngle4f getRotationDown(float[] someNormal){
		
		float epsilon = 0.0001f;
		
		Vector3f curr = new Vector3f(someNormal);
		Vector3f down = new Vector3f(0,-1,0);
		curr.normalize();
		down.normalize();
		
		Vector3f up = new Vector3f(0,1,0);
		
		
		if(curr.epsilonEquals(down, epsilon)){
			return new AxisAngle4f();
		} else if(curr.epsilonEquals(up, epsilon)){
			return new AxisAngle4f(1f,0f,0f,(float)Math.PI);
		}
		
		Vector3f rotVec = new Vector3f();
		rotVec.cross(curr, down);
		rotVec.normalize();
		
		float angle = curr.angle(down);
		
		AxisAngle4f rotAxAng = new AxisAngle4f(rotVec.x,rotVec.y,rotVec.z,angle);
		
		if(Double.isNaN(rotVec.x)){
			System.out.println("Curr "+curr);
			System.out.println("down: "+down);
		}
		
		return rotAxAng;
	}
		
	@Override
	public void setCurrentTransform(float[] result, MatrixOrder mo) {
		
		currentTransform = Arrays.copyOf(result, 16);
		if(mo==MatrixOrder.COLUMN_MAJOR) MatrixOrder.convertOrder(currentTransform, 0, currentTransform, 0);
			
		this.currentMatrix.set(currentTransform);		
	}
	
	@Override
	public void getCurrentTransform(float[] result, MatrixOrder mo){
		//Log.d("TranTest","Getting Transform with Matrix Order: "+mo);
		for(int i=0;i<16;i++) result[i]=currentTransform[i];
		//Log.d("TranTest","Pre conversion: "+Arrays.toString(result));
		if(mo==MatrixOrder.COLUMN_MAJOR) MatrixOrder.convertOrder(result, 0, result, 0);
		//Log.d("TranTest","Post conversion: "+Arrays.toString(result));
	}
	
	private void getDoubleArrayFromMatrix(Matrix4f source, float[] result){
		for(int i=0;i<4;i++){
			for (int j=0;j<4;j++){
				result[i*4+j]=currentMatrix.getElement(i,j);
			}
		}		
	}

	@Override
	public void pointNormalDown(float[] normal) {
		AxisAngle4f rotAxAng = getRotationDown(normal);
		this.currentMatrix.setRotation(rotAxAng);
		this.getDoubleArrayFromMatrix(currentMatrix, currentTransform);
	}

	@Override
	public void transformPoint(float[] point) {
		Point3f thePoint = new Point3f(point);
		this.currentMatrix.transform(thePoint);
		thePoint.get(point);		
	}

	@Override
	public void setScale(float scale) {
		this.currentMatrix.setScale(scale);
		this.getDoubleArrayFromMatrix(currentMatrix, currentTransform);
	}

	@Override
	public float getScale() {
		return this.currentMatrix.getScale();
	}

}